#include "RigidBody.h"
#include "ConvertUtil.h"
#include <string>

using namespace std;

extern string model;

void State::setState( RigidBody* b )
{
	pos = b->pos;
	vel = b->vel;
	rotation = b->rotation;
	orientation = b->orientation;
}

RigidBody::RigidBody( void )
{
	density = 1.0;
	initRigidBody(conversionFromObj);
}

RigidBody::RigidBody( string filename, double density, double res )
{
	this->density = density;
	this->res = res;
	model = filename;
	printf("%s\n", filename.c_str());
	initRigidBody(conversionFromObj);
}

RigidBody::~RigidBody( void )
{

}

void RigidBody::initRigidBody(initializeFunc setUp)
{
	this->bounciness = 1.0;
	this->friction = 0.0;	

	this->frozen = false;

	this->pos = Vec3d(0, 0, 0);
	this->vel = Vec3d(0, 0, 0);
	this->orientation = Quaterniond(1, 0, 0, 0);
	this->rotation = Vec3d(0, 0, 0);	
	this->collisionGraph = new vector<CollisionPair*>;

	poly = new Polyhedron();
	poly->buildPolyStructure(setUp);	

	computeMassProperty();

	this->inverseInertiaTensor = inverse(bodyInertiaMatrix);
	this->inverseMassMatrix = Mat33d(1.0/mass, 0, 0, 0, 1.0/mass, 0, 0, 0, 1/mass);

	poly->translatePolyhedron(-1.0 * Vec3d(centerX, centerY, centerZ));

	sdm = new GridSDM(poly, res);

	radius = 0.0;

	for (int i=0; i<poly->vertCount; i++)
	{
		Vec3d vert(poly->vertList[i].x, poly->vertList[i].y, poly->vertList[i].z);

		radius = max(radius, mag(vert));
	}

	rebuildMatrices();
}

void RigidBody::computeMassProperty()
{
	volume = 0;		

	centerX = centerY = centerZ = 0.0;

	for (int i=0; i<9; i++)
	{
		inertiaTensorAtRest[i] = 0;
	}


	for (int i=0; i<poly->faceCount; i++)
	{
		Tri tf = poly->getFaceFromFace(i);

		int ia, ib, ic;

		ia = tf.a;
		ib = tf.b;
		ic = tf.c;

		Vert tva = poly->vertList[ia];
		Vert tvb = poly->vertList[ib];
		Vert tvc = poly->vertList[ic];

		double determinant = getDeterminant(tva.x, tva.y, tva.z, tvb.x, tvb.y, tvb.z, tvc.x, tvc.y, tvc.z);

		if (determinant < 0)
		{
			Vert swap;

			swap = tva;
			tva = tvb;
			tvb = swap;
		}

		addTriangleContribution(tva.x, tva.y, tva.z, tvb.x, tvb.y, tvb.z, tvc.x, tvc.y, tvc.z);
	}

	double r = 1.0 / (4 * volume);
	centerX *= r;
	centerY *= r;
	centerZ *= r;

	volume /= 6.0;	

	r = 1.0 / 120;

	inertiaTensorAtRest[1] = inertiaTensorAtRest[1] * r - volume * centerY * centerX;
	inertiaTensorAtRest[2] = inertiaTensorAtRest[2] * r - volume * centerZ * centerX;
	inertiaTensorAtRest[5] = inertiaTensorAtRest[5] * r - volume * centerY * centerZ;

	inertiaTensorAtRest[0] = inertiaTensorAtRest[0] * r - volume * centerX * centerX;
	inertiaTensorAtRest[4] = inertiaTensorAtRest[4] * r - volume * centerY * centerY;
	inertiaTensorAtRest[8] = inertiaTensorAtRest[8] * r - volume * centerZ * centerZ;

	double xx, yy, zz;

	xx = inertiaTensorAtRest[0];
	yy = inertiaTensorAtRest[4];
	zz = inertiaTensorAtRest[8];

	inertiaTensorAtRest[0] = yy + zz;
	inertiaTensorAtRest[4] = zz + xx;
	inertiaTensorAtRest[8] = xx + yy;	

	mass = density * volume;	
	bodyInertiaMatrix = density * Mat33d(inertiaTensorAtRest[0], inertiaTensorAtRest[1], inertiaTensorAtRest[2], inertiaTensorAtRest[1], inertiaTensorAtRest[4], inertiaTensorAtRest[5], inertiaTensorAtRest[2], inertiaTensorAtRest[5], inertiaTensorAtRest[8]);

}

double RigidBody::getDeterminant( double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3 )
{
	double v = x1*y2*z3 + y1*z2*x3 + x2*y3*z1 -
		(x3*y2*z1 + x2*y1*z3 + y3*z2*x1);
	return v;
}

void RigidBody::addTriangleContribution( double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3 )
{
	double v = x1*y2*z3 + y1*z2*x3 + x2*y3*z1 -
		(x3*y2*z1 + x2*y1*z3 + y3*z2*x1);

	// Contribution to the mass
	volume += v;

	// Contribution to the centroid
	double x4 = x1 + x2 + x3;           centerX += (v * x4);
	double y4 = y1 + y2 + y3;           centerY += (v * y4);
	double z4 = z1 + z2 + z3;           centerZ += (v * z4);

	// Contribution to moment of inertia monomials
	inertiaTensorAtRest[0] += v * (x1*x1 + x2*x2 + x3*x3 + x4*x4);
	inertiaTensorAtRest[4] += v * (y1*y1 + y2*y2 + y3*y3 + y4*y4);
	inertiaTensorAtRest[8] += v * (z1*z1 + z2*z2 + z3*z3 + z4*z4);
	inertiaTensorAtRest[1] += v * (y1*x1 + y2*x2 + y3*x3 + y4*x4);
	inertiaTensorAtRest[2] += v * (z1*x1 + z2*x2 + z3*x3 + z4*x4);
	inertiaTensorAtRest[5] += v * (z1*y1 + z2*y2 + z3*y3 + z4*y4);   
}

void RigidBody::rebuildMatrices()
{
	double* temp = (double*)malloc(9 * sizeof(double));

	orientation.Quaternion2Matrix(temp);

	Mat33d newOrientation(temp);

	delete temp;

	orientationMatrix = newOrientation.transpose();

	inverseOrientationMatrix = orientationMatrix.transpose();

	inverseWorldInertiaMatrix = orientationMatrix * inverseInertiaTensor * inverseOrientationMatrix;
}

Vec3d RigidBody::getWorldVelocity( Vec3d x )
{
	rebuildMatrices();

	return vel + cross(rotation, x - pos);
}

Vec3d RigidBody::getPointWorldToBody( Vec3d p )
{
	rebuildMatrices();

	return inverseOrientationMatrix * (p - pos);
}

Vec3d RigidBody::getPointBodyToWorld( Vec3d p )
{
	rebuildMatrices();

	return orientationMatrix * p + pos;
}

Vec3d RigidBody::getVectorWorldToBody( Vec3d v )
{
	rebuildMatrices();

	return inverseOrientationMatrix * v;
}

Vec3d RigidBody::getVectorBodyToWorld( Vec3d v )
{
	rebuildMatrices();

	return orientationMatrix * v;
}

void RigidBody::setState( State src )
{
	this->pos = src.pos;
	this->vel = src.vel;
	this->orientation = src.orientation;
	this->rotation = src.rotation;
}

Value RigidBody::evaluateSDM( Vec3d p, bool compute_normal )
{
	return sdm->getEvaluation(p, compute_normal);
}

Mat33d RigidBody::computeK( Vec3d x )
{	
	if (frozen || mass > 1e8)
	{
		return Mat33d(0, 0, 0, 0, 0, 0, 0, 0, 0);
	}

	rebuildMatrices();

	Mat33d Rx = star_matrix(x - pos);

	return inverseMassMatrix - Rx * inverseWorldInertiaMatrix * Rx;
}

void RigidBody::updatePos( double dt )
{
	pos += vel * dt;
	double rotSpeed = mag(rotation);

	if (rotSpeed > 1e-8)
	{
		Vec3d vtemp = rotation * (1.0 / rotSpeed);
		double qin[3] = {vtemp[0], vtemp[1], vtemp[2]};
		Quaterniond temp(rotSpeed * dt, qin);
		orientation = orientation * temp;
	}
}

void RigidBody::updateVel( double dt )
{
	if (mass < 1e8)
	{
		vel += dt * inverseMassMatrix * netForce;
		rotation += dt * inverseWorldInertiaMatrix * netTorque;
	}
}

void RigidBody::applyImpulse( Vec3d x, Vec3d J )
{
	if (frozen || mass > 1e8)
	{
		return;
	}

	if (!(-1e8 < J[0] && J[0] < 1e8 && -1e8 < J[1] && J[1] < 1e8 && -1e8 < J[2] && J[2] < 1e8))
	{
		printf("what the fuck impulse\n");
		return;
	}

	rebuildMatrices();

	Vec3d Rx = x - pos;
	Vec3d vOld = vel;
	Vec3d vNew = vOld + (1.0 / mass) * J;
	Vec3d wOld = rotation;
	Vec3d wNew = wOld + inverseWorldInertiaMatrix *cross(Rx, J);

	vel = vNew;
	rotation = wNew;
}

void RigidBody::applyForce( Vec3d x, Vec3d f )
{
	if (frozen || mass > 1e8)
	{
		return;
	}

	if (!(-1e8 < f[0] && f[0] < 1e8 && -1e8 < f[1] && f[1] < 1e8 && -1e8 < f[2] && f[2] < 1e8))
	{
		printf("what the fuck impulse\n");
		return;
	}

	rebuildMatrices();

	netForce += f;

	Vec3d Rx = x - pos;

	netTorque += cross(Rx, f);
}

void RigidBody::resetForce()
{
	netForce = Vec3d(0, 0, 0);
	netTorque = Vec3d(0, 0, 0);
}
